#include <memory>
#include <thread>
#include "nav2_msgs/action/navigate_to_pose.hpp"  // 导入导航动作消息的头文件
#include "rclcpp/rclcpp.hpp"                      // 导入ROS 2的C++客户端库
#include "rclcpp_action/rclcpp_action.hpp"        // 导入ROS 2的C++ Action客户端库
#include <tf2_ros/transform_listener.h>
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/buffer.h"
#include <geometry_msgs/msg/pose2_d.hpp>
#include <geometry_msgs/msg/polygon.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/point32.hpp>

// 定义导航动作类型为NavigateToPose
using NavigationAction = nav2_msgs::action::NavigateToPose;
// 定义导航动作客户端类型
using NavigationActionClient = rclcpp_action::Client<NavigationAction>;
bool is_goal = false;
bool is_finish_nav = false;


class RoomExplorationServer
{
public:
    RoomExplorationServer(rclcpp::Node::SharedPtr &nh);

    void GoalResultCallback(const rclcpp_action::ClientGoalHandle<NavigationAction>::WrappedResult &result);

    void runss();

protected:

    rclcpp::Node::SharedPtr node_handle_;
    NavigationActionClient::SharedPtr action_client_;
};

RoomExplorationServer::RoomExplorationServer(rclcpp::Node::SharedPtr &nh) : node_handle_(nh)
{
    action_client_ = rclcpp_action::create_client<NavigationAction>(node_handle_, "navigate_to_pose");
    std::thread first(&RoomExplorationServer::runss, this);
    first.detach();
}

void RoomExplorationServer::GoalResultCallback(
        const rclcpp_action::ClientGoalHandle<NavigationAction>::WrappedResult &result)
{
    switch (result.code)
    {
        case rclcpp_action::ResultCode::SUCCEEDED:
            is_goal = true;
            std::cout << "处理成功！" << std::endl;
            break;
        case rclcpp_action::ResultCode::ABORTED:
            std::cout << "处理失败！" << std::endl;
            break;
        case rclcpp_action::ResultCode::CANCELED:
            std::cout << "取消了！" << std::endl;
            break;
        case rclcpp_action::ResultCode::UNKNOWN:
            std::cout << "未知的！ UNKNOWN " << std::endl;
            break;
        default:
            std::cout << "未知的！" << std::endl;
            break;
    }
    is_finish_nav = true;
}

void RoomExplorationServer::runss()
{
    // 等待导航动作服务器上线，等待时间为5秒
    while (!action_client_->wait_for_action_server(std::chrono::seconds(5)))
    {
        RCLCPP_INFO(node_handle_->get_logger(), "等待Action服务上线。");
    }
    // 设置导航目标点
    auto goal_msg = NavigationAction::Goal();
    goal_msg.pose.header.stamp = rclcpp::Time();
    goal_msg.pose.header.frame_id = "map";  // 设置目标点的坐标系为地图坐标系
    goal_msg.pose.pose.position.x = -4.0;   // 设置目标点的x坐标
    goal_msg.pose.pose.position.y = 2;      // 设置目标点的y坐标
    auto send_goal_options =
            rclcpp_action::Client<NavigationAction>::SendGoalOptions();
    // 设置执行结果回调函数
    send_goal_options.result_callback =
            std::bind(&RoomExplorationServer::GoalResultCallback, this, std::placeholders::_1);
    // 发送导航目标点
    RCLCPP_INFO(node_handle_->get_logger(), "---------------- async_send_goal pub ----------------");
    auto result = action_client_->async_send_goal(goal_msg, send_goal_options);
    rclcpp::Rate loop_rate(10);
    int loop_cut = 0;
    while (rclcpp::ok())
    {
        if (is_finish_nav)
        {
            std::cout << "is_finish_nav: " << is_finish_nav << std::endl;
            if (is_goal)break;
            else
            {
                std::cout << "is_goal: " << is_goal << std::endl;
                loop_cut++;
                if (loop_cut < 2)
                {
                    action_client_->async_send_goal(goal_msg, send_goal_options);
                } else break;
            }
        }
        loop_rate.sleep();
//        rclcpp::spin_some(node_handle_);
    }
    RCLCPP_INFO(node_handle_->get_logger(), "---------------- async_send_goal finish ----------------");
}

int main(int argc, char **argv)
{
//    rclcpp::init(argc, argv);
//    auto node = rclcpp::Node::make_shared("nav_to_pose_client");
//    RoomExplorationServer test_navi(node);
//    std::cout << "------------" << std::endl;
//    rclcpp::Rate spin_rate(10);
//    while (rclcpp::ok()) spin_rate.sleep();
//    rclcpp::spin(node);
//    rclcpp::shutdown();

    auto beginTime = std::chrono::high_resolution_clock::now();
    std::this_thread::sleep_for(std::chrono::seconds(2));
    auto endTime = std::chrono::high_resolution_clock::now();
    double programTimes = std::chrono::duration_cast<std::chrono::seconds>(endTime - beginTime).count();
    std::cout << "programTimes: " << programTimes << std::endl;
    return 0;
}

